#include <iostream>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/core/core.hpp>
#include "opencv_lidar.h"
#include "opencv2/opencv.hpp"

using namespace cv;
using std::vector;


// Scalar leftLow=Scalar(137,59,71),leftHigh=Scalar(192,255,255); //blue yilou bangwan all_lights_on 0.25 0.035 rgb
// Scalar rightLow=Scalar(62,122,82),rightHigh=Scalar(130,255,255); //red yilou bangwan all_lights_on  0.25 0.035 rgb
// Scalar rightLow=Scalar(62,64,0),rightHigh=Scalar(122,255,100); //蓝 一楼 下午 自动曝光 hsv
// Scalar leftLow=Scalar(0,152,28),leftHigh=Scalar(10,255,255); //红 一楼 下午 自动曝光 hsv
// Scalar rightLow=Scalar(66,98,0),rightHigh=Scalar(110,255,66); //蓝 一楼 下午 自动曝光 hsv newzhuitong
// Scalar leftLow=Scalar(0,167,57),leftHigh=Scalar(15,255,202); //红 一楼 下午 自动曝光 hsv newzhuitong

Scalar leftLow=Scalar(125,117,88),leftHigh=Scalar(255,255,255); //zhujiao
Scalar leftLow2=Scalar(0,117,88),leftHigh2=Scalar(20,255,255); //zhujiao
Scalar rightLow=Scalar(100,100,0),rightHigh=Scalar(175,255,255); //zhujiao
// Scalar rightLow=Scalar(65,41,24),rightHigh=Scalar(111,255,255);//1f
// left绿框，right黄框

// Scalar rightLow=Scalar(71,53,0),rightHigh=Scalar(134,255,255);//1f
// Scalar leftLow=Scalar(125,53,0),leftHigh=Scalar(255,255,255); //zhujiao
// Scalar leftLow2=Scalar(0,53,0),leftHigh2=Scalar(20,255,255); //zhujiao

Mat element = getStructuringElement(MORPH_RECT, Size(5, 5) ); //矩形的5*5内核
ConeInfo cone_judge(Mat image);
bool convex_hull_pointing_up(vector<Point> contour);

ConeInfo ConeDetect::img_convert(Mat img_src,int color)
{
    Mat img_rgb;
    Mat img_HSV;
    Mat temp1,temp2;
    Mat left,right,end,temp;
    Mat img_thresh_opened,img_thresh_blurred,img_edges;
    ConeInfo Contours;
    Contours.amount = 0;
    vector<vector<Point> > color_contours;
    vector<Vec4i> hierarchy;

    cvtColor(img_src, img_HSV, CV_BGR2HSV);
    
    //二值化，在两阈值范围内的点变白色，其余为黑色。
    if(color == LEFTCONE)
    {
        inRange(img_HSV, leftLow, leftHigh, temp1);
        inRange(img_HSV, leftLow2, leftHigh2, temp2);
        temp = temp1 + temp2;
    }
    else if(color == RIGHTCONE)
    {
        inRange(img_HSV, rightLow, rightHigh, temp);
    }
    //图像融合
    // addWeighted(left, 0.5, right, 0.5, 0.0, temp);
    morphologyEx(temp, img_thresh_opened, 2, element); //开操作，（输入，输出，开，核），貌似是去除较亮的噪点
    medianBlur(img_thresh_opened, img_thresh_opened, 5); //中值滤波
    Canny(img_thresh_opened, img_edges, 20, 80, 3);  //边缘检测
    // cv::imshow("canny",img_edges);
    findContours(img_edges, color_contours, hierarchy, CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE, Point(0, 0));
    for( int id = 0; id < color_contours.size(); id++ )
    {
    	Rect rect;
		rect = boundingRect(color_contours[id]);
        // ROS_INFO("rect.area:%d",rect.area());
    	if(rect.area() > 700)
    	{    		
			Contours.x.push_back(rect.x);
			Contours.y.push_back(rect.y);
			Contours.width.push_back(rect.width);
			Contours.height.push_back(rect.height);
            Contours.hulls_cone.push_back(color_contours[Contours.amount]);
            Contours.amount++;
    	}
    }     
    return Contours; //每个锥桶的图像区域
}

ConeInfo ConeDetect::cone_convert(Mat img_src,int coneJudge)
{
    Mat img_rgb;
    Mat img_HSV;
    Mat left1,left2;
    Mat left,right,end;
    ConeInfo Contours;

    // cvtColor(img_src, img_rgb, CV_BGR2RGB);
    cvtColor(img_src, img_HSV, CV_BGR2HSV);
    // cv::imshow("img_HSV",img_HSV);
    // cv::waitKey(-1);
    //二值化，在两阈值范围内的点变白色，其余为黑色。
    inRange(img_HSV, leftLow, leftHigh, left1);
    inRange(img_HSV, leftLow2, leftHigh2, left2);
    left = left1 + left2;
    // inRange(img_rgb, leftLow, leftHigh, left);
    // cv::imshow("left|cone_convert",left);
    // cv::waitKey(-1);
    inRange(img_HSV, rightLow, rightHigh, right);
    // inRange(img_rgb, rightLow, rightHigh, right);
    // cv::imshow("right|cone_convert",right);
    // cv::waitKey(-1);
    // inRange(img_HSV, Scalar(endLowH, endLowS, endLowV), Scalar(endHighH, endHighS, endHighV), end);
    
    if(coneJudge == LEFTCONE)
    {
        Contours = cone_judge(left);
    }
    else if(coneJudge == RIGHTCONE)
    {
        Contours = cone_judge(right);
    }
    // else if(coneJudge == ENDCONE)
    // {
    //     Contours = cone_judge(end);
    // }
    
    return Contours;
}

ConeInfo cone_judge(Mat image)
{
    Mat img_all_convex_hulls;
    vector<vector<Point> > contours;
    vector<Vec4i> hierarchy;
    ConeInfo temp_cone;
    temp_cone.amount = 0;
    Mat img_thresh_opened,img_thresh_blurred,img_edges;

    morphologyEx(image, img_thresh_opened, 2, element); //开
    medianBlur(img_thresh_opened, img_thresh_blurred, 5); //中值滤波
    Canny(img_thresh_blurred, img_edges, 20, 80, 3);   //边缘检测
    findContours(img_edges, contours, hierarchy, CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE, Point(0, 0));
    vector<vector<Point> >approx_contours(contours.size());
    vector<vector<Point> >all_convex_hulls(contours.size());
    vector<vector<Point> >convex_hulls_3to10(contours.size());
    vector<vector<Point> >convex_hulls_cone(contours.size());

    for( int i = 0; i < contours.size(); i++ )
    {
         approxPolyDP( Mat(contours[i]), approx_contours[i], 3, true );
    }
    for( int i = 0; i < approx_contours.size(); i++ )
    {
         convexHull( Mat(approx_contours[i]), all_convex_hulls[i], false );
    }
    for( int i = 0; i< all_convex_hulls.size(); i++ )
    {
         if(all_convex_hulls[i].capacity() >= 3 && all_convex_hulls[i].capacity() <= 10)
         {
           convexHull( Mat(all_convex_hulls[i]), convex_hulls_3to10[i], false );
         }
    }
    
   for( int i = 0; i < convex_hulls_3to10.size(); i++ )
    {
        if(convex_hull_pointing_up(convex_hulls_3to10[i]))
        {
            // ROS_INFO("entered IF");
            Rect rect;
            // rect = pointSetbboundingRect(Mat(convex_hulls_3to10[i]));
            rect = boundingRect(convex_hulls_3to10[i]);
            temp_cone.x.push_back(rect.x);
            temp_cone.width.push_back(rect.width);
            temp_cone.y.push_back(rect.y);
            temp_cone.height.push_back(rect.height);
            convexHull( Mat(convex_hulls_3to10[i]), convex_hulls_cone[temp_cone.amount], false );
            temp_cone.hulls_cone.push_back(convex_hulls_cone[temp_cone.amount]);
            temp_cone.amount++;
        }
    }
    return temp_cone;
}

bool convex_hull_pointing_up(vector<Point> contour)
{
    vector<int> points_above_center(10);
    vector<int> points_below_center(10);
    Rect boundRect;
    // ROS_INFO("convex_hull_pointing_up start");
    boundRect = boundingRect(contour);
    // ROS_INFO("pointSetbboundingRect finished!!");
    int up_index = 0;
    int down_index = 0;
    int left_x = 0;
    int right_x = 0;
    float aspect_ratio = static_cast<float>((boundRect.width / 1.0) / boundRect.height);
    // ROS_INFO("convex_hull_pointing_up begins to judge");
    if(boundRect.area() < 800)
    {
        return false;
    }
    else if(aspect_ratio < 0.8)
    {
        float vertical_center = boundRect.y + cvRound(boundRect.height/2.0);

        for(int i = 0; i < contour.capacity(); i++)
        {
            if(contour[i].y < vertical_center)
            {
                points_above_center[up_index] = contour[i].x;
                up_index++;
            }
            else if(contour[i].y >= vertical_center)
            {
                points_below_center[down_index] = contour[i].x;
                down_index++;
            }
        }
        left_x = points_below_center[down_index-1];
        right_x = points_below_center[down_index-1];

        for(int i = 0; i < down_index; i++)
        {
            if(points_below_center[i] < left_x) //寻找最小值
            {
                left_x = points_below_center[i];
            }
            if(points_below_center[i] > right_x) //寻找最大值
            {
                right_x = points_below_center[i];
            }
        }
        int up_left_x = points_above_center[up_index-1];
        int up_right_x = points_above_center[up_index-1];

         for(int i = 0; i < up_index; i++)
        {
            if(points_above_center[i] < up_left_x) //寻找最小值
            {
                up_left_x = points_above_center[i];
            }
            if(points_above_center[i] > up_right_x) //寻找最大值
            {
                up_right_x = points_above_center[i];
            }
            if(points_above_center[i] < left_x || points_above_center[i] > right_x)
            {
                return false;
            }
        }
    }
    else{
        return false;
    }
    return true;
}

